import json
import os
from typing import Dict, List, Any, Optional
import math

try:
    from gglobal import logger
except ImportError:
    import logging
    logger = logging.getLogger(__name__)


def load_map_data(map_file_path: str) -> Optional[Dict[str, Any]]:
    """
    加载地图数据文件
    
    Args:
        map_file_path: 地图文件路径
        
    Returns:
        地图数据字典，如果加载失败返回None
    """
    try:
        # 清理路径中的引号
        map_file_path = map_file_path.strip('"').strip("'")
        
        if not os.path.exists(map_file_path):
            logger.error(f"地图文件不存在: {map_file_path}")
            return None
            
        with open(map_file_path, 'r', encoding='utf-8') as f:
            map_data = json.load(f)
            
        logger.info(f"成功加载地图文件: {map_file_path}")
        return map_data
        
    except json.JSONDecodeError as e:
        logger.error(f"解析地图文件JSON格式失败: {e}")
        return None
    except Exception as e:
        logger.error(f"加载地图文件失败: {e}")
        return None


def parse_nodes_from_map(map_data: Dict[str, Any]) -> Dict[int, Dict[str, Any]]:
    """
    从地图数据中解析节点信息
    
    Args:
        map_data: 地图数据字典
        
    Returns:
        节点ID到节点信息的映射
    """
    nodes = {}
    
    try:
        lane_marks = map_data.get("mLaneMarks", [])
        
        for lane_mark in lane_marks:
            node_id = lane_mark.get("mLaneMarkID")
            node_name = lane_mark.get("mLaneMarkName", "")
            xyzw = lane_mark.get("mLaneMarkXYZW", {})
            
            if node_id is not None:
                node = {
                    "id": node_id,
                    "name": node_name,
                    "x": xyzw.get("x", 0.0),
                    "y": xyzw.get("y", 0.0),
                    "z": xyzw.get("z", 0.0),
                    "w": xyzw.get("w", 1.0),
                    "type": lane_mark.get("mLaneMarkType", 0)
                }
                nodes[node_id] = node
                
        logger.debug(f"解析到 {len(nodes)} 个节点")
        return nodes
        
    except Exception as e:
        logger.error(f"解析节点信息失败: {e}")
        return {}


def parse_lanes_from_map(map_data: Dict[str, Any], nodes: Dict[int, Dict[str, Any]]) -> List[Dict[str, Any]]:
    """
    从地图数据中解析车道信息
    
    Args:
        map_data: 地图数据字典
        nodes: 节点信息字典
        
    Returns:
        车道信息列表
    """
    lanes = []
    
    try:
        roads = map_data.get("mRoads", [])
        
        for road in roads:
            road_lanes = road.get("mLanes", [])
            
            for lane in road_lanes:
                lane_id = lane.get("mLaneID")
                start_pos = lane.get("mStartPos")
                end_pos = lane.get("mEndPos")
                lane_type = lane.get("mLaneType", 0)  # 0直线, 1曲线
                direction = lane.get("mDirection", 0)  # 0双向, 1正单向, 2逆单向
                lane_area = lane.get("mLanePro", 0)  # 0主路, 1取货通道, 2放货通道
                length = lane.get("mLength", 0.0)
                
                if lane_id is not None and start_pos is not None and end_pos is not None:
                    if start_pos == end_pos:
                        continue  # 跳过起点终点相同的车道
                        
                    # 获取起点和终点节点信息
                    start_node = nodes.get(start_pos)
                    end_node = nodes.get(end_pos)
                    
                    if start_node and end_node:
                        # 生成车道点
                        points = generate_lane_points(lane, start_node, end_node)
                        
                        lane_info = {
                            "id": lane_id,
                            "start_id": start_pos,
                            "end_id": end_pos,
                            "type": lane_type,
                            "direction": direction,
                            "area": lane_area,
                            "length": length,
                            "points": points
                        }
                        lanes.append(lane_info)
                        
        logger.debug(f"解析到 {len(lanes)} 条车道")
        return lanes
        
    except Exception as e:
        logger.error(f"解析车道信息失败: {e}")
        return []


def generate_lane_points(lane: Dict[str, Any], start_node: Dict[str, Any], end_node: Dict[str, Any]) -> List[Dict[str, float]]:
    """
    生成车道点序列
    
    Args:
        lane: 车道信息
        start_node: 起点节点
        end_node: 终点节点
        
    Returns:
        车道点列表
    """
    points = []
    lane_type = lane.get("mLaneType", 0)
    
    try:
        # 添加起点
        start_point = {
            "x": start_node["x"],
            "y": start_node["y"],
            "z": 0.0
        }
        points.append(start_point)
        
        if lane_type == 0:  # 直线车道
            # 直线插值中间点
            kLanePointInterval = 0.5  # 点间隔
            length = lane.get("mLength", 0.0)
            
            if length > 0:
                num = max(1, int(math.ceil(length / kLanePointInterval)))
                dx = (end_node["x"] - start_node["x"]) / num
                dy = (end_node["y"] - start_node["y"]) / num
                
                for i in range(1, num):
                    point = {
                        "x": start_node["x"] + i * dx,
                        "y": start_node["y"] + i * dy,
                        "z": 0.0
                    }
                    points.append(point)
        else:  # 曲线车道
            # 使用曲线路径点
            data_path = lane.get("mDataPath", [])
            for path_point in data_path:
                point = {
                    "x": path_point.get("x", 0.0),
                    "y": path_point.get("y", 0.0),
                    "z": 0.0
                }
                points.append(point)
        
        # 添加终点
        end_point = {
            "x": end_node["x"],
            "y": end_node["y"],
            "z": 0.0
        }
        points.append(end_point)
        
        # 计算每个点的yaw角
        calculate_point_yaw(points, lane_type)
        
        return points
        
    except Exception as e:
        logger.error(f"生成车道点失败: {e}")
        return []


def calculate_point_yaw(points: List[Dict[str, float]], lane_type: int):
    """
    计算车道点的yaw角
    
    Args:
        points: 车道点列表
        lane_type: 车道类型 (0直线, 1曲线)
    """
    try:
        if len(points) < 2:
            return
            
        if lane_type == 0:  # 直线
            # 所有点使用相同的yaw角
            if len(points) >= 2:
                dx = points[-1]["x"] - points[0]["x"]
                dy = points[-1]["y"] - points[0]["y"]
                yaw = math.atan2(dy, dx)
                
                for point in points:
                    point["yaw"] = yaw
        else:  # 曲线
            # 每个点根据相邻点计算yaw角
            for i in range(len(points)):
                if i == 0:
                    if len(points) > 1:
                        dx = points[1]["x"] - points[0]["x"]
                        dy = points[1]["y"] - points[0]["y"]
                        points[i]["yaw"] = math.atan2(dy, dx)
                    else:
                        points[i]["yaw"] = 0.0
                elif i == len(points) - 1:
                    dx = points[i]["x"] - points[i-1]["x"]
                    dy = points[i]["y"] - points[i-1]["y"]
                    points[i]["yaw"] = math.atan2(dy, dx)
                else:
                    dx = points[i+1]["x"] - points[i-1]["x"]
                    dy = points[i+1]["y"] - points[i-1]["y"]
                    points[i]["yaw"] = math.atan2(dy, dx)
                    
    except Exception as e:
        logger.error(f"计算点yaw角失败: {e}")


def convert_map_to_marker_array(map_file_path: str, header: Dict[str, Any]) -> Dict[str, Any]:
    """
    将地图数据转换为MarkerArray格式
    
    Args:
        map_file_path: 地图文件路径
        header: ROS消息头信息
        
    Returns:
        转换后的MarkerArray数据
    """
    try:
        # 加载地图数据
        map_data = load_map_data(map_file_path)
        if not map_data:
            return {"markers": []}
            
        # 解析节点和车道
        nodes = parse_nodes_from_map(map_data)
        lanes = parse_lanes_from_map(map_data, nodes)
        
        # 创建MarkerArray
        marker_array = {"markers": []}
        
        # 为每条车道创建一个LINE_STRIP marker
        for i, lane in enumerate(lanes):
            points = lane.get("points", [])
            if len(points) < 2:
                continue
                
            # 创建marker
            marker = {
                "header": {
                    "stamp": header.get("stamp", {"sec": 0, "nanosec": 0}),
                    "frame_id": "map"
                },
                "ns": "map_lines",
                "id": i,
                "type": 4,  # LINE_STRIP
                "action": 0,  # ADD
                "pose": {
                    "position": {"x": 0.0, "y": 0.0, "z": 0.0},
                    "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}
                },
                "scale": {"x": 0.1, "y": 0.0, "z": 0.0},  # 线宽
                "color": get_lane_color(lane),
                "lifetime": {"sec": 0, "nanosec": 0},  # 永久显示
                "points": [{"x": p["x"], "y": p["y"], "z": p["z"]} for p in points],
                "colors": [],
                "text": f"Lane_{lane['id']}"
            }
            
            marker_array["markers"].append(marker)
            
        logger.info(f"成功转换地图为MarkerArray，包含 {len(marker_array['markers'])} 个marker")
        return marker_array
        
    except Exception as e:
        logger.error(f"转换地图为MarkerArray失败: {e}")
        return {"markers": []}


def get_lane_color(lane: Dict[str, Any]) -> Dict[str, float]:
    """
    根据车道类型获取颜色
    
    Args:
        lane: 车道信息
        
    Returns:
        颜色字典
    """
    lane_area = lane.get("area", 0)
    lane_type = lane.get("type", 0)
    
    if lane_area == 0:  # 主路
        return {"r": 0.0, "g": 1.0, "b": 0.0, "a": 0.8}  # 绿色
    elif lane_area == 1:  # 取货通道
        return {"r": 0.0, "g": 0.0, "b": 1.0, "a": 0.8}  # 蓝色
    elif lane_area == 2:  # 放货通道
        return {"r": 1.0, "g": 0.0, "b": 0.0, "a": 0.8}  # 红色
    else:
        return {"r": 1.0, "g": 1.0, "b": 0.0, "a": 0.8}  # 黄色


def process_map_to_marker_array(msg_dict: Dict[str, Any], localmap_path: str) -> Dict[str, Any]:
    """
    处理/map话题的数据转换功能
    
    Args:
        msg_dict: 原始/map消息字典
        localmap_path: 本地地图文件路径
        
    Returns:
        转换后的MarkerArray数据
    """
    try:
        # 清理路径中的引号
        localmap_path = localmap_path.strip('"').strip("'")
        logger.debug(f"开始处理/map数据转换，使用地图文件: {localmap_path}")
        
        # 提取header信息
        header = msg_dict.get("header", {})
        
        # 转换为MarkerArray格式
        marker_array_data = convert_map_to_marker_array(localmap_path, header)
        
        return marker_array_data
        
    except Exception as e:
        logger.error(f"处理/map数据转换时出错: {e}")
        return {"markers": []}